{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "0",
   "metadata": {},
   "source": [
    "# Get images from cameras"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2",
   "metadata": {},
   "source": [
    "Reachy 2 has 2 types of camera:\n",
    "\n",
    "- the **teleop** cameras, with a right and left cameras, located in Reachy 2’s head and used for the teleoperation\n",
    "- the **depth** camera, equipped with a depth sensor, located in Reachy 2’s torso and mainly useful for manipulation tasks\n",
    "\n",
    "Each camera can be accessed separately through reachy.cameras. Teleop cameras  have a right and left view, with the left and right sides considered from Reachy point of view, while the depth camera has a left (i.e. mono RGB) and depth view. To be able to specify the view you want to get a frame from, you will need to import CameraView:"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3",
   "metadata": {},
   "source": [
    "```python\n",
    "from reachy2_sdk.media.camera import CameraView\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4",
   "metadata": {},
   "source": [
    "## Get images"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5",
   "metadata": {},
   "source": [
    "First, connect to your robot.  \n",
    "**Do not forget to import the CameraView!**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6",
   "metadata": {},
   "outputs": [],
   "source": [
    "from reachy2_sdk.media.camera import CameraView\n",
    "from reachy2_sdk import ReachySDK\n",
    "\n",
    "reachy = ReachySDK(host='localhost')  # Replace with the actual IP"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7",
   "metadata": {},
   "source": [
    "Check the list of initialized cameras:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.cameras"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9",
   "metadata": {},
   "source": [
    "The list of initialized cameras should contain both the teleop and depth cameras.  "
   ]
  },
  {
   "cell_type": "markdown",
   "id": "10",
   "metadata": {},
   "source": [
    "### Teleop cameras\n",
    "\n",
    "To get both views of the robot teleop cameras:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "11",
   "metadata": {},
   "outputs": [],
   "source": [
    "l_frame, l_ts = reachy.cameras.teleop.get_frame(CameraView.LEFT)\n",
    "r_frame, r_ts = reachy.cameras.teleop.get_frame(CameraView.RIGHT)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "12",
   "metadata": {},
   "source": [
    "We can print the timestamp of each frame (in nanosecond)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "13",
   "metadata": {},
   "outputs": [],
   "source": [
    "print(f\"timestamp left frame {l_ts} - timestamp right frame {r_ts}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "14",
   "metadata": {},
   "source": [
    "Let's display the captured frame with PIL:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "15",
   "metadata": {},
   "outputs": [],
   "source": [
    "from PIL import Image"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "16",
   "metadata": {},
   "outputs": [],
   "source": [
    "Image.fromarray(l_frame[:,:,::-1])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "18",
   "metadata": {},
   "source": [
    "The camera parameters, as defined [here](https://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/CameraInfo.html), are also available"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "19",
   "metadata": {},
   "outputs": [],
   "source": [
    "height, width, distortion_model, D, K, R, P =  reachy.cameras.teleop.get_parameters(CameraView.LEFT)\n",
    "print(f\"height: {height}\")\n",
    "print(f\"width: {width}\")\n",
    "print(f\"distortion model: {distortion_model}\")\n",
    "print(f\"distortion coefficients {D}\")\n",
    "print(f\"instrinsic matrix {K}\")\n",
    "print(f\"rectification matrix {R}\")\n",
    "print(f\"projection matrix {P}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "20",
   "metadata": {},
   "source": [
    "### Depth camera\n",
    "\n",
    "The depth camera works exactly the same as the teleop camera, but you have more elements captured. In fact, it's a RGBD camera, so you have both access to the RGB image and depth information.\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "21",
   "metadata": {},
   "source": [
    "#### RGB images\n",
    "\n",
    "Getting RGB images from the depth camera looks the same as from the teleop one:  simply use `get_frame()`, there is only one view."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "22",
   "metadata": {},
   "outputs": [],
   "source": [
    "frame, ts = reachy.cameras.depth.get_frame()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "23",
   "metadata": {},
   "source": [
    "Let's display the captured frame with PIL:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "24",
   "metadata": {},
   "outputs": [],
   "source": [
    "Image.fromarray(frame[:,:,::-1])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "25",
   "metadata": {},
   "source": [
    "As for the teleop camera, parameters are also availables"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "26",
   "metadata": {},
   "outputs": [],
   "source": [
    "height, width, distortion_model, D, K, R, P =  reachy.cameras.depth.get_parameters()\n",
    "print(f\"height: {height}\")\n",
    "print(f\"width: {width}\")\n",
    "print(f\"distortion model: {distortion_model}\")\n",
    "print(f\"distortion coefficients {D}\")\n",
    "print(f\"instrinsic matrix {K}\")\n",
    "print(f\"rectification matrix {R}\")\n",
    "print(f\"projection matrix {P}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "27",
   "metadata": {},
   "source": [
    "#### Depth information\n",
    "\n",
    "The SR camera is a depth camera, you can then diplay a left or right **depth frame** using `get_depth_frame()`, but also the **depthmap** and the **disparity**.   \n",
    "\n",
    "You first have to capture all, then you can read the frame and get the information you want:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "28",
   "metadata": {},
   "outputs": [],
   "source": [
    "depth_frame, ts = reachy.cameras.depth.get_depth_frame()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "29",
   "metadata": {},
   "source": [
    "Let's display the captured frame with PIL:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "30",
   "metadata": {},
   "outputs": [],
   "source": [
    "Image.fromarray(depth_frame[:,:])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "31",
   "metadata": {},
   "source": [
    "If needed, camera parameters for the depth view are also available"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "32",
   "metadata": {},
   "outputs": [],
   "source": [
    "height, width, distortion_model, D, K, R, P =  reachy.cameras.depth.get_parameters(CameraView.DEPTH)\n",
    "print(f\"height: {height}\")\n",
    "print(f\"width: {width}\")\n",
    "print(f\"distortion model: {distortion_model}\")\n",
    "print(f\"distortion coefficients {D}\")\n",
    "print(f\"instrinsic matrix {K}\")\n",
    "print(f\"rectification matrix {R}\")\n",
    "print(f\"projection matrix {P}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "33",
   "metadata": {},
   "source": [
    "## Live stream"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "34",
   "metadata": {},
   "source": [
    "Although we provide an optimal way to get the video stream, it is still possible to display what Reachy see through the SDK. It could be useful to feed a compute vision algorithm that do not need to run at high frequency.\n",
    "\n",
    "This is demonstrated in a dedicated script : [cameras.py](cameras.py)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
